%roser2.m
%takes gmax,fov,nres,gam,
%computes k-trajectory kss for simulation tests
%D Twieg 8/06
%modified by mbolding@uab.edu 9/07
%                         ____              
%  _ __ ___  ___  ___ _ _|___ \   _ __ ___  
% | '__/ _ \/ __|/ _ \ '__|__) | | '_ ` _ \ 
% | | | (_) \__ \  __/ |  / __/ _| | | | | |
% |_|  \___/|___/\___|_| |_____(_)_| |_| |_|
%                                           
% play around with values in the lead-in -out section to tweak the initial
% and final sections of the trajectory. The gradients should start and end
% near zero as should the k-trajectory

% which trajectory
% wtraj = 'pre15';  % Spline or Trapazoid for now

% hardware
MAX_DAC_VAL=32767.0;% DAC max integer, corresponds to gro, gpe
GDELT=1e-05;        % gradient waveform DAC speed

% constants
gam=4258.;
twopi=2*pi;
sq2=sqrt(2); 

%defaults
circum=1;  % circumscribed (=1) or not (!=1).
gmax=3.8; % maximum read gradient to be used.
pre_emph=1.05; % pre emphasis to compensate for high freq attenuation
fov=12.8;  % field of view in cm
nres=102;   % resolution
kf=nres/(2*fov); 
wtraj = 't66';

% other parameters of desired trajectory
switch lower(wtraj)
    case 't65'
        Ta=.065;   % gradient duration in seconds without lead-in and -out
    case 't66'
        Ta=0.066;   % gradient duration in seconds without lead-in and -out
    case 't15'
        Ta=.015;   % gradient duration in seconds without lead-in and -out
    case 't30'
        Ta=.030;   % gradient duration in seconds without lead-in and -out
    case 'test'
        Ta=.030;   % gradient duration in seconds without lead-in and -out
    case 't120'
        Ta=.120;   % gradient duration in seconds without lead-in and -out
    otherwise
        error('please set wtraj')
end


delt=1./(gam*gmax*fov); % ADC sample rate
Na=floor(Ta/delt); % number of acqusitions
tim=(-50:Na-1)*delt; % vector of time points
% generate trajectory
if circum == 1     % circumscribed or not
    kra=sqrt(2)*kf;
    Ne=2*round(sqrt(2)*nres/2);
else
    kra=kf;
    Ne=nres;
end

om1=gam*gmax/kra;
om2=(41.1/nres)*om1;

% generate vector of gradient points
gs=kra*(pre_emph*(om1+om2)*(cos((om1+om2)*tim+pi/2)+1i*sin((om1+om2)*tim+pi/2))...
  + (om2-om1)*(cos((om2-om1)*tim+pi/2)+1i*sin((om2-om1)*tim+pi/2)))/(2*gam);

% generate lead-in and lead-out
% play with the internal points of the trapezoids or splines to get the
% lead-in and -out correct.
num_pts = 100;
pts = 1:num_pts;
switch lower(wtraj)
    case 't65'
        L = [ 1.545+1i*(-0.99091)   3.03+1i*(-0.89752) ;    %lead in tweakers
              1.555+1i*(0.85)     -0.6+1i*(-0.8)];          %lead out tweakers
        ctl_pts_x = [0 num_pts/3  2*num_pts/3 num_pts];
        gs_li = spline(ctl_pts_x,[ 0       L(1,1)  L(1,2)   gs(1) ],pts);     % lead-in
        gs_lo = spline(ctl_pts_x,[ gs(end) L(2,1)  L(2,2)   0     ],pts);     % lead-out
    case 't66'
        L = [ 1.545+1i*(-0.99091)   3.03+1i*(-0.89752) ;    %lead in tweakers
              -1.2+1i*(-2)     -0.1+1i*(-0.6)];             %lead out tweakers
        ctl_pts_x = [0 num_pts/3  2*num_pts/3 num_pts];
        gs_li = spline(ctl_pts_x,[ 0       L(1,1)  L(1,2)   gs(1) ],pts);     % lead-in
        gs_lo = spline(ctl_pts_x,[ gs(end) L(2,1)  L(2,2)   0     ],pts);     % lead-out
    case 't15'
        L = [ 2.045+1i*(-1.59091)   2.53+1i*(0.70248) ;      %lead in tweakers
              -1.745+1i*(0.83)     -2.9+1i*(-1.4)];          %lead out tweakers
        ctl_pts_x = [0 num_pts/3  2*num_pts/3 num_pts];
        gs_li = spline(ctl_pts_x,[ 0       L(1,1)  L(1,2)   gs(1) ],pts);     % lead-in
        gs_lo = spline(ctl_pts_x,[ gs(end) L(2,1)  L(2,2)   0     ],pts);     % lead-out
    case 't30'
        L = [ 1.6+1i*(-0.9)   2.9+1i*(-0.8) ;               %lead in tweakers
              -2.4+1i*(0.73)     -2.32+1i*(-2.3)];          %lead out tweakers
        ctl_pts_x = [0 num_pts/3  2*num_pts/3 num_pts];
        gs_li = spline(ctl_pts_x,[ 0       L(1,1)  L(1,2)   gs(1) ],pts);     % lead-in
        gs_lo = spline(ctl_pts_x,[ gs(end) L(2,1)  L(2,2)   0     ],pts);     % lead-out
    case 'test'
        L = [ 2.045+1i*(-1.59091)   2.53+1i*(0.70248) ;     %lead in tweakers
              -1.445+1i*(0.13)     0.1+1i*(-1.4)];          %lead out tweakers
        ctl_pts_x = [0 num_pts/3  2*num_pts/3 num_pts];
        gs_li = spline(ctl_pts_x,[ 0       L(1,1)  L(1,2)   gs(1) ],pts);     % lead-in
        gs_lo = spline(ctl_pts_x,[ gs(end) L(2,1)  L(2,2)   0     ],pts);     % lead-out
    case 't120'
        L = [ 1.545+1i*(-0.99091)   3.03+1i*(-0.89752) ;    %lead in tweakers
              -3.6+1i*(0.5)     -0.5+1i*(-3.5)];            %lead out tweakers
        ctl_pts_x = [0 num_pts/3  2*num_pts/3 num_pts];
        gs_li = spline(ctl_pts_x,[ 0       L(1,1)  L(1,2)   gs(1) ],pts);     % lead-in
        gs_lo = spline(ctl_pts_x,[ gs(end) L(2,1)  L(2,2)   0     ],pts);     % lead-out
   otherwise
      error('Unknown method. Check lead in and out code.')
end
gs = [gs_li gs gs_lo]; % put the lead-in -out and gradient wave together
gs_max=max(abs(gs));

% predict k trajectory vector by integrating gs
ks2=kra+gam*delt*cumsum(gs);
ks2=ks2-ks2(1);  % assume start at zero

% compute resampled and scaled output vectors for UnityInova hardware
Q=10000; % we resample at P/Q rate, P, Q must be pos. int.
P=round(Q*delt/GDELT); % downsample because grad DAC is slower than ADC
dac = gs*MAX_DAC_VAL/gs_max; %use computed max dac value  not gmax (problem?)
dac_r=resample(real(dac),P,Q); % actual values sent to hardware
dac_i=resample(imag(dac),P,Q); 


% show that puppy
len_d=100;    % number of points to display on lead-in and -out dac
len_d_k=900; % number of points to display on lead-in and -out ktraj

figure(1)

subplot(3,3,1); % plot dac
plot(dac_i,dac_r); 
title(['dac gs_m_a_x=' num2str(gs_max)]);

subplot(3,3,2); % dac start
plot(dac_r(1:len_d),dac_i(1:len_d),'o',...
    real(dac(1:round(len_d*Q/P))),...
    imag(dac(1:round(len_d*Q/P)))); 
hold on
plot(L(1,:)*MAX_DAC_VAL/gs_max,'r+')
hold off
title('dac start ');

subplot(3,3,3); % dac end
plot(dac_r(end-len_d+1:end),dac_i(end-len_d+1:end),'o'); 
hold on
plot(L(2,:)*MAX_DAC_VAL/gs_max,'r+')
hold off
title('dac end ');

% subplot(3,3,2); % dac start
% plot(1:len_d,dac_r(1:len_d),'o',1:len_d,dac_i(1:len_d),'o',...
%     (1:round(len_d*Q/P))*P/Q,real(dac(1:round(len_d*Q/P))),...
%     (1:round(len_d*Q/P))*P/Q,imag(dac(1:round(len_d*Q/P)))); 
% title(['dac start ' num2str(gs_max)]);
% 
% subplot(3,3,3); % dac end
% plot(1:len_d,dac_r(end-len_d+1:end),'+',1:len_d,dac_i(end-len_d+1:end),'+'); 
% title(['dac end ' num2str(gs_max)]);

subplot(3,3,4); % plot ks
plot(ks2)
title(['ks ' wtraj])

zm=[-1 1 -1 1]*0.82646; % zoom in to center
subplot(3,3,5); % ks start
plot(ks2(1:len_d_k))
hold on
plot(ks2(1:2),'+r')
hold off
title(['ks start ' num2str(ks2(1))])
axis(zm)

subplot(3,3,6); % ks end
plot(ks2(end-len_d_k:end))
hold on
plot(ks2(end-2:end),'+r')
hold off
title(['ks end ' num2str(ks2(end))])
axis(zm)

ks2_diff=[ks2 ks2(end)]-[ks2(1) ks2];
subplot(3,3,7) % ks diff
plot(ks2_diff(1:len_d_k))
hold on
plot(ks2_diff(end-len_d_k+1:end),'g')
hold off

subplot(3,3,8); % ks start
plot(ks2(1:len_d_k),'o')
hold on
plot(ks2(1:len_d_k/10),'r')
hold off
title('ks start')

subplot(3,3,9); % ks end
plot(ks2(end-len_d_k+1:end),'o')
hold on
plot(ks2(end-len_d_k/10:end),'r')
hold off
title('ks end')

% subplot(3,3,8);
% plot(1:len_d_k,real(ks2(1:len_d_k)),'+',1:len_d_k,imag(ks2(1:len_d_k)),'+')
% title('ks start')
% 
% subplot(3,3,9);
% plot(1:len_d_k,real(ks2(end-len_d_k+1:end)),'+',1:len_d_k,imag(ks2(end-len_d_k+1:end)),'+')
% title('ks end')




% %roser2a.m
% %takes gmax,fov,nres,gam,
% %computes k-trajectory kss for simulation tests
% %(i.e., trajectory does not include wind-up and wind-down sections required in actual
% %implementation)
% %D Twieg 8/06
% %constants
% gam=4258.;twopi=2*pi;sq2=sqrt(2);
% 
% %parameters of desired trajectory
% gmax=4.5;%maximum read gradient to be used.
% fov=12.8;%field of view
% nres=64;%resolution
% circum=1;%circumscribed (1) or not (not 1).
% Ta=.065;%duration in seconds
% 
% kf=nres/(2*fov);
% delt=1./(gam*gmax*fov);
% Na=floor(Ta/delt);
% tim=(0:Na-1)*delt;
% 
% if circum == 1
%     kra=sqrt(2)*kf;
%     Ne=2*round(sqrt(2)*nres/2);
% else
%     kra=kf;
%     Ne=nres;
% end
% 
% om1=gam*gmax/kra;
% om2=(41/nres)*om1;
% gs=kra*(1.05*(om1+om2)*(cos((om1+om2)*tim+pi/2)+1i*sin((om1+om2)*tim+pi/2))...
%     + (om2-om1)*(cos((om2-om1)*tim+pi/2)+1i*sin((om2-om1)*tim+pi/2)))/(2*gam);
% ks2=kra+gam*delt*cumsum(gs);


